#include "rc.h"
#include "que.h"
#include "MeOrion.h"

#define ON 1
#define OFF 0

#define LOST_COUNT 10000

#define MAX(val1,val2) (val1>val2?val1:val2)
#define MIN(val1,val2) (val1<val2?val1:val2)
#define LIMIT(val,bound1,bound2) (val=val>MAX(bound1,bound2)?MAX(bound1,bound2):(val<MIN(bound1,bound2)?MIN(bound1,bound2):val))

#define BIN_ON_SERVO_ANGLE 130    //仓门开时舵机角度
#define BIN_OFF_SERVO_ANGLE 85    //仓门关时舵机角度
#define AIR_ON_SERVO_ANGLE 88     //风道开时舵机角度
#define AIR_OFF_SERVO_ANGLE 0     //风道关时舵机角度
#define WRIST_SERVO_ANGLE_MAX 180 //手腕舵机最大角度
#define WRIST_SERVO_ANGLE_MIN 0   //手腕舵机最小角度
#define WRIST_SERVO_ANGLE_OFF 90  //手腕舵机复位角度
#define WRIST_SERVO_ANGLE_COM 15  //手腕舵机角度补偿
#define FAN_ON_SERVO_ANGLE 150    //涵道开启对应角度
#define FAN_OFF_SERVO_ANGLE 45    //涵道停止对应角度
#define DC_MOTOR_SPEED_MAX 255    //直流电机最大速度
#define DC_MOTOR_SPEED_MIN -255   //直流电机最小速度

#define ARM_DC_MOTOR1_DIR_COEFF 1       //手臂直流电机1方向系数
#define ARM_DC_MOTOR2_DIR_COEFF -1      //手臂直流电机2方向系数
#define CLAW_DC_MOTOR2_DIR_COEFF 1      //爪子直流电机方向系数

MePort port_6(PORT_6);            //控制口
MePort port_7(PORT_7);            //控制口
MePort port_3(PORT_3);            //控制口
MePort port_4(PORT_4);            //控制口

int16_t fan_servo_pin =  port_6.pin1();//涵道控制口
int16_t air_servo_pin =  port_3.pin2();//风道控制口
int16_t bin_servo_pin =  port_3.pin1();//舱门控制口
int16_t wrist_servo_pin1 = port_4.pin1();//手腕控制口1
int16_t wrist_servo_pin2 = port_4.pin2();//手腕控制口2

MeDCMotor arm_dc_motor1(PORT_1);  //手臂直流电机1
MeDCMotor arm_dc_motor2(PORT_2);  //手臂直流电机2
MeDCMotor claw_dc_motor(M2);      //爪子直流电机

Servo fan_servo;                 //涵道PWM
Servo air_servo;                 //风道舵机
Servo bin_servo;                 //舱门舵机
Servo wrist_servo1;              //手腕舵机1
Servo wrist_servo2;              //手腕舵机2

int fan_state = OFF;                          //涵道运行状态
int air_state = OFF;                          //风道开关状态
int bin_state = OFF;                          //仓门开关状态
int arm_motor_speed = 0;                      //手臂电机速度
int wrist_servo_angle = WRIST_SERVO_ANGLE_OFF;//手腕舵机角度
int claw_motor_speed = 0;                     //爪子电机速度

Que* que = NULL;                //消息队列
RC rc;                          //遥控器数据
uint32_t lost_counter = 0;      //看门狗计数器

#define SWITCH_ACTION_NONE 0
#define SWITCH_ACTION_3TO1 ((3<<2)|1)
#define SWITCH_ACTION_1TO3 ((1<<2)|3)
#define SWITCH_ACTION_3TO2 ((3<<2)|2)
#define SWITCH_ACTION_2TO3 ((2<<2)|3)

int switchAction = 0;
int this_s1 = 0;
int last_s1 = 0;
void GetSwitchAction(void)
{
  last_s1 = this_s1;
  this_s1 = rc.s1;
  switchAction = (last_s1<<2)|this_s1;
}

void GetFanState(void)
{
  fan_state = (switchAction == SWITCH_ACTION_3TO1) ? (!fan_state) : fan_state;
}

void FanAction(void)
{
  if(fan_state == ON)
    fan_servo.write(FAN_ON_SERVO_ANGLE);
  else
    fan_servo.write(FAN_OFF_SERVO_ANGLE);
}

void GetAirState(void)
{
  air_state = (switchAction == SWITCH_ACTION_3TO1) ? (!air_state) : air_state;
}

void AirAction(void)
{
  if(air_state == ON)
    air_servo.write(AIR_ON_SERVO_ANGLE);
  else
    air_servo.write(AIR_OFF_SERVO_ANGLE);
}

void GetBinState(void)
{
  bin_state = (switchAction == SWITCH_ACTION_3TO2) ? (!bin_state) : bin_state;
}

void  BinAction(void)
{
  if(bin_state == ON)
    bin_servo.write(BIN_ON_SERVO_ANGLE);
  else
    bin_servo.write(BIN_OFF_SERVO_ANGLE);
}

void GetArmSpeed(void)
{
  arm_motor_speed = map(rc.ch1, CH_MIN, CH_MAX, DC_MOTOR_SPEED_MIN, DC_MOTOR_SPEED_MAX);
}

void ArmAction(void)
{
  LIMIT(arm_motor_speed, DC_MOTOR_SPEED_MIN, DC_MOTOR_SPEED_MAX);
  arm_dc_motor1.run(arm_motor_speed * ARM_DC_MOTOR1_DIR_COEFF);
  arm_dc_motor2.run(arm_motor_speed * ARM_DC_MOTOR2_DIR_COEFF);
}

void GetWristAngle(void)
{
  wrist_servo_angle += (rc.ch3 - CH_MID) / 150.f;
}

void WristAction(void)
{
  LIMIT(wrist_servo_angle, WRIST_SERVO_ANGLE_MIN, WRIST_SERVO_ANGLE_MAX);
  wrist_servo1.write(wrist_servo_angle);
  wrist_servo2.write(wrist_servo_angle + WRIST_SERVO_ANGLE_COM); //补偿舵机特性差异
}

void GetClawSpeed(void)
{
  claw_motor_speed = map(rc.ch2, CH_MIN, CH_MAX, DC_MOTOR_SPEED_MIN, DC_MOTOR_SPEED_MAX);
}

void ClawAction(void)
{
  LIMIT(claw_motor_speed, DC_MOTOR_SPEED_MIN, DC_MOTOR_SPEED_MAX);
  claw_dc_motor.run(claw_motor_speed * CLAW_DC_MOTOR2_DIR_COEFF);
}

void ParseRemoteCmd(void)
{
  GetSwitchAction();
  GetFanState();
  GetAirState();
  GetBinState();
  if(rc.s2 == SW_UP)
  {
    GetArmSpeed();
    GetWristAngle();
    GetClawSpeed();
  }
}

void Action(void)
{
  FanAction();
  AirAction();
  BinAction();
  ArmAction();
  WristAction();
  ClawAction();
}

void AppInit(void)
{
  this_s1 = 0;
  last_s1 = 0;
  switchAction = SWITCH_ACTION_NONE;
  
  fan_state = OFF;
  air_state = OFF;
  bin_state = OFF;
  
  arm_motor_speed = 0;
  wrist_servo_angle = WRIST_SERVO_ANGLE_OFF;
  claw_motor_speed = 0;
 
  lost_counter = 0;
}

void SuperviseTask(void)
{
  if(lost_counter < LOST_COUNT) lost_counter++;
  if(lost_counter >= LOST_COUNT)
  {
    AppInit();
    Action();
  }
}

void setup() 
{
  que = QueCreate(RC_FRAME_SIZE);
  fan_servo.attach(fan_servo_pin);    //涵道电调初始化
  air_servo.attach(air_servo_pin);    //风道舵机初始化
  bin_servo.attach(bin_servo_pin);    //仓门舵机初始化
  wrist_servo1.attach(wrist_servo_pin1);//手腕舵机1初始化
  wrist_servo2.attach(wrist_servo_pin2);//手腕舵机2初始化
  
  AppInit();
  Serial.begin(115200);
}

void loop() 
{ 
  if(Serial.available())
  {
    QueIn(que, Serial.read());
    if(RC_Pop(&rc, que))
    {
        lost_counter = 0;
        ParseRemoteCmd();
        Action();
        //Debug();
    }
  }
  SuperviseTask();
}

void Debug(void)
{
  Serial.print("fan_state=");
  Serial.print(fan_state);
  Serial.print(",");
  Serial.print("air_state=");
  Serial.print(air_state);
  Serial.print(",");
  Serial.print("bin_state=");
  Serial.print(bin_state);
  Serial.print(",");
  Serial.print("arm_motor_speed=");
  Serial.print(arm_motor_speed);
  Serial.print(",");
  Serial.print("wrist_servo_angle=");
  Serial.print(wrist_servo_angle);
  Serial.print(",");
  Serial.print("claw_motor_speed=");
  Serial.println(claw_motor_speed);
}

